package ma.fsm.arda.command;

import ma.fsm.arda.serial.ArdaSerial4Arduino;

import org.sintef.jarduino.AnalogPin;
import org.sintef.jarduino.DigitalPin;
import org.sintef.jarduino.DigitalState;
import org.sintef.jarduino.PWMPin;
import org.sintef.jarduino.PinMode;

public class Command {

	// Motor A definitions
	final DigitalPin DIR_A = DigitalPin.PIN_12;
	final PWMPin SPEED_A = PWMPin.PWM_PIN_3;
	final DigitalPin BREAK_A = DigitalPin.PIN_9;

	// Motor B definitions
	final DigitalPin DIR_B = DigitalPin.PIN_13;
	final PWMPin SPEED_B = PWMPin.PWM_PIN_11;
	final DigitalPin BREAK_B = DigitalPin.PIN_8;
	// LED definitions
	final DigitalPin LED_B = DigitalPin.PIN_2;
	final DigitalPin LED_R = DigitalPin.PIN_7;

	// Sensors definitions

	final AnalogPin sensorPinA = AnalogPin.A_4;
	final AnalogPin sensorPinB = AnalogPin.A_5;

	private JarduinoDataController controller;

	private String state = "outOfService";
	private boolean auto = false;

	public String getState() {
		return state;
	}

	public void setState(String state) {
		this.state = state;
	}

	public void setAuto(boolean auto) {
		this.auto = auto;
	}

	public boolean isAuto() {
		return auto;
	}

	public Command(String port) {

		// Serial4JArduino device = new Serial4JArduino(port);
		ArdaSerial4Arduino device = new ArdaSerial4Arduino(port);
		System.out.println(device.toString());
		controller = new JarduinoDataController(device);
		device.register(controller);
		System.out.println("start setup");
		// setup();
		controller.sendpinMode(PinMode.OUTPUT, LED_B);
		controller.sendpinMode(PinMode.OUTPUT, LED_R);
		state = "started";
	}

	protected void loop() {
		// TODO Auto-generated method stub

	}

	protected void setup() {

		controller.sendpinMode(PinMode.OUTPUT, DIR_A);
		controller.sendpinMode(PinMode.OUTPUT, DIR_B);
		controller.sendpinMode(PinMode.OUTPUT, BREAK_A);
		controller.sendpinMode(PinMode.OUTPUT, BREAK_B);

		// release break
		controller.senddigitalWrite(BREAK_B, DigitalState.HIGH);
		// release break
		controller.senddigitalWrite(BREAK_A, DigitalState.HIGH);
	}

	public synchronized void stop() {
		if (!state.equals("stop")) {

			controller.sendanalogWrite(SPEED_B, (byte) 0);
			controller.sendanalogWrite(SPEED_A, (byte) 0);

			System.out.println("stop");
			state = "stop";
		}
	}

	public synchronized void goForward() {
		if (!state.equals("forward")) {
			controller.senddigitalWrite(DIR_B, DigitalState.LOW);
			controller.sendanalogWrite(SPEED_B, (byte) 250);
			controller.senddigitalWrite(DIR_A, DigitalState.LOW);
			controller.sendanalogWrite(SPEED_A, (byte) 250);
			state = "forward";
		}

	}

	public synchronized void goLeft() {
		if (!state.equals("left")) {
			controller.senddigitalWrite(DIR_B, DigitalState.LOW);
			controller.sendanalogWrite(SPEED_B, (byte) 100);
			controller.senddigitalWrite(DIR_A, DigitalState.LOW);
			controller.sendanalogWrite(SPEED_A, (byte) 200);
			state = "left";
		}
	}

	public synchronized void spinLeft() {
		if (!state.equals("sleft")) {
			controller.senddigitalWrite(DIR_B, DigitalState.HIGH);
			controller.sendanalogWrite(SPEED_B, (byte) 100);
			controller.senddigitalWrite(DIR_A, DigitalState.LOW);
			controller.sendanalogWrite(SPEED_A, (byte) 100);
			state = "sleft";
		}
	}

	public synchronized void goRight() {
		if (!state.equals("right")) {

			controller.senddigitalWrite(DIR_B, DigitalState.LOW);
			controller.sendanalogWrite(SPEED_B, (byte) 200);
			controller.senddigitalWrite(DIR_A, DigitalState.LOW);
			controller.sendanalogWrite(SPEED_A, (byte) 100);
			state = "right";

		}
	}

	public synchronized void spinRight() {
		if (!state.equals("sright")) {
			controller.senddigitalWrite(DIR_B, DigitalState.LOW);
			controller.sendanalogWrite(SPEED_B, (byte) 100);
			controller.senddigitalWrite(DIR_A, DigitalState.HIGH);
			controller.sendanalogWrite(SPEED_A, (byte) 100);
			state = "sright";

		}
	}

	public synchronized void goBackward() {
		if (!state.equals("backward")) {
			controller.senddigitalWrite(DIR_B, DigitalState.HIGH);
			controller.sendanalogWrite(SPEED_B, (byte) 250);
			controller.senddigitalWrite(DIR_A, DigitalState.HIGH);
			controller.sendanalogWrite(SPEED_A, (byte) 250);
			state = "backward";

		}

	}

	public void onBlue() {
		controller.senddigitalWrite(LED_B, DigitalState.HIGH);

	}

	public void offBlue() {
		controller.senddigitalWrite(LED_B, DigitalState.LOW);

	}

	public void onRed() {
		controller.senddigitalWrite(LED_R, DigitalState.HIGH);

	}

	public void offRed() {
		controller.senddigitalWrite(LED_R, DigitalState.LOW);

	}

	public short sensorA() {
		return controller.sendanalogRead(sensorPinA);

	}

	public short sensorB() {
		return controller.sendanalogRead(sensorPinB);

	}

}
